#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2_eigen/tf2_eigen.hpp> // 用于 Eigen 和 Pose 之间的转换
#include <thread>
#include <cmath> // ‼️ 添加，用于 M_PI

// 定义一个辅助函数，用于在单独的线程中启动执行器
// (MoveGroupInterface 的构造函数需要这个)
void spin_node(std::shared_ptr<rclcpp::Node> node) {
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
}

int main(int argc, char** argv) {
    // 1. 初始化 ROS 2
    rclcpp::init(argc, argv);

    // =================================================================
    // =========== ‼️ 新增：解析命令行参数 ‼️ ============================
    // =================================================================
    
    // 检查参数数量是否正确 (1个程序名 + 5个关节角)
    if (argc != 6) {
        // 使用一个临时的 logger，因为 node 还没有创建
        RCLCPP_ERROR(rclcpp::get_logger("fk_solver_node"), 
                     "错误：需要 5 个关节角度作为参数。");
        RCLCPP_INFO(rclcpp::get_logger("fk_solver_node"), 
                    "用法: %s <j1_deg> <j2_deg> <j3_deg> <j4_deg> <j5_deg>", argv[0]);
        rclcpp::shutdown();
        return 1;
    }

    // 存储用户输入的“度”
    std::vector<double> joint_values_degrees;
    try {
        for (int i = 1; i <= 5; ++i) {
            joint_values_degrees.push_back(std::stod(argv[i]));
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("fk_solver_node"), 
                     "参数解析错误: '%s'. 请确保只输入数字。", e.what());
        rclcpp::shutdown();
        return 1;
    }

    // =================================================================
    // =========== ‼️ 参数解析结束 ‼️ ===================================
    // =================================================================

    // 2. 创建节点实例
    auto node = std::make_shared<rclcpp::Node>("fk_solver_node",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
    
    // 3. 在单独的线程中运行节点执行器
    std::thread spin_thread(spin_node, node);

    // 4. 定义规划组
    static const std::string PLANNING_GROUP = "zc4";

    // 5. 初始化 MoveGroupInterface
    RCLCPP_INFO(node->get_logger(), "Loading MoveGroupInterface for group '%s'...", PLANNING_GROUP.c_str());
    moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP);
    RCLCPP_INFO(node->get_logger(), "MoveGroupInterface loaded.");

    // 6. 获取核心组件
    moveit::core::RobotModelConstPtr robot_model = move_group.getRobotModel();
    std::shared_ptr<moveit::core::RobotState> robot_state = 
        std::make_shared<moveit::core::RobotState>(robot_model);
    const moveit::core::JointModelGroup* joint_model_group = 
        robot_model->getJointModelGroup(PLANNING_GROUP);

    // 7. 获取并验证关节信息
    const std::vector<std::string>& joint_names = joint_model_group->getActiveJointModelNames();

    std::string names_str;
    for (const auto& name : joint_names) {
        names_str += name + ", ";
    }
    RCLCPP_INFO(node->get_logger(), "Joints in group '%s' are: [ %s ]", 
        PLANNING_GROUP.c_str(), names_str.c_str());

    // 验证关节数量是否与输入匹配
    if (joint_names.size() != 5) {
        RCLCPP_ERROR(node->get_logger(), 
            "错误: 规划组 'zc4' 的关节数 (%zu) 不等于 5. 无法继续。", 
            joint_names.size());
        rclcpp::shutdown();
        spin_thread.join();
        return 1;
    }

    // 8. 获取末端执行器 (End-Effector) 连杆
    std::string eef_link = move_group.getEndEffectorLink();
    if (eef_link.empty()) {
        RCLCPP_ERROR(node->get_logger(), 
            "错误: 规划组 '%s' 没有在 SRDF 中定义末端连杆。 无法继续。", 
            PLANNING_GROUP.c_str());
        rclcpp::shutdown();
        spin_thread.join();
        return 1;
    }
    RCLCPP_INFO(node->get_logger(), "End-effector link is: '%s'", eef_link.c_str());


    // =================================================================
    // =========== ‼️ 修改：从“度”转换为“弧度” ‼️ ========================
    // =================================================================
    
    // 这里的顺序必须 严格对应 上面打印出的 "Joints in group" 的顺序
    std::vector<double> joint_values; // 存储弧度值
    for (double degrees : joint_values_degrees) {
        joint_values.push_back(degrees * M_PI / 180.0);
    }

    // 打印日志以供验证
    std::string deg_str, rad_str;
    for (size_t i = 0; i < 5; ++i) {
        deg_str += std::to_string(joint_values_degrees[i]) + ", ";
        rad_str += std::to_string(joint_values[i]) + ", ";
    }
    RCLCPP_INFO(node->get_logger(), "Input (Degrees): [ %s ]", deg_str.c_str());
    RCLCPP_INFO(node->get_logger(), "Input (Radians): [ %s ]", rad_str.c_str());
    
    // =================================================================
    

    // 9. 设置关节状态
    RCLCPP_INFO(node->get_logger(), "Setting joint values...");
    robot_state->setJointGroupPositions(joint_model_group, joint_values);
    robot_state->update(); // 更新所有连杆的变换

    // 10. 解算正运动学 (FK)
    // 获取末端连杆的全局位姿 (Eigen::Isometry3d 格式)
    const Eigen::Isometry3d& eef_pose_eigen = robot_state->getGlobalLinkTransform(eef_link);

    // 11. 转换并输出结果
    // 将 Eigen::Isometry3d 转换为 geometry_msgs::msg::Pose
    geometry_msgs::msg::Pose output_pose = tf2::toMsg(eef_pose_eigen);

    RCLCPP_INFO(node->get_logger(), "=== Forward Kinematics (FK) Result ===");
    RCLCPP_INFO(node->get_logger(), "Position (x, y, z):");
    RCLCPP_INFO(node->get_logger(), "  x: %.6f", output_pose.position.x);
    RCLCPP_INFO(node->get_logger(), "  y: %.6f", output_pose.position.y);
    RCLCPP_INFO(node->get_logger(), "  z: %.6f", output_pose.position.z);
    RCLCPP_INFO(node->get_logger(), "Orientation (qx, qy, qz, qw):");
    RCLCPP_INFO(node->get_logger(), "  qx: %.6f", output_pose.orientation.x);
    RCLCPP_INFO(node->get_logger(), "  qy: %.6f", output_pose.orientation.y);
    RCLCPP_INFO(node->get_logger(), "  qz: %.6f", output_pose.orientation.z);
    RCLCPP_INFO(node->get_logger(), "  qw: %.6f", output_pose.orientation.w);
    RCLCPP_INFO(node->get_logger(), "========================================");

    // 12. 清理
    RCLCPP_INFO(node->get_logger(), "Shutting down node.");
    rclcpp::shutdown();
    spin_thread.join(); // 等待 spin 线程结束
    return 0;
}